# tracking - By: starry - 周五 2月 17 2023




import sensor, image, time,screen,button
from os import mkdir
from pyb import UART,millis
from text import font

#s2 = Servo(2) # P8 Servo,
#s2.pulse_width(1500)
sensor.reset()
sensor.set_framesize(sensor.QQVGA)
#sensor.set_windowing(160,86)
sensor.set_pixformat(sensor.RGB565)
sensor.set_vflip(True)
sensor.set_hmirror(True)

clock = time.clock()

degrees=0

screen=screen.screen()

face_cascade = image.HaarCascade("frontalface", stages=25)

img_grayscale = sensor.alloc_extra_fb(160,86, sensor.GRAYSCALE)
image_data = sensor.alloc_extra_fb(50,50, sensor.GRAYSCALE)

image_matching = sensor.alloc_extra_fb(50,50, sensor.GRAYSCALE)
face_size=[]


uart = UART(1,115200)
#uart.write("tracking start\n")
uart_bytes=0

while True:
    while True:
        uart_read=uart.readchar()
        print(uart_read)
        if uart_read==88 and  uart_read_last==84 : #received "TX"
            print("received TX\n")
            #uart.write("TX\n")
            break
        uart_read_last=uart_read

        img = sensor.snapshot()
        image_data.draw_image(img,0,0,roi=(55,18,50,50))
        #img.draw_text(font, 160, 20, 'Target Track', color=(250,120,120),scale=2, center_text=True)
        img.draw_text(font, 50, 0, '目标追踪')
        img.draw_rectangle(55,18,50,50,thickness=2)
        screen.display(img,x_size=320)
        time.sleep_ms(1000)
    #录入
    while True:
        #s2.pulse_width(1500)
        img = sensor.snapshot()
        sensor.ioctl(sensor.IOCTL_TRIGGER_AUTO_FOCUS)#自动对焦
        timer=millis()
        while millis()-timer<1000:
            img = sensor.snapshot()
            img.draw_rectangle(55,18,50,50,color=(255,255,0),thickness=2)
            screen.display(img,x_size=320)
        img = sensor.snapshot()
        image_data.draw_image(img,0,0,roi=(55,18,50,50))

        img.draw_rectangle(55,18,50,50,thickness=2)
        screen.display(img,x_size=320)
        break
    #识别
    while True:
        img = sensor.snapshot()
        img_grayscale.draw_image(img,0,0)
        img_grayscale.to_grayscale()
        results = img_grayscale.find_template(image_data, 0.70, step=2, search=image.SEARCH_EX)#匹配率70%追踪
        if results:
            image_matching.draw_image(img_grayscale,0,0,roi=results)
            if image_matching.find_template(image_data, 0.85, step=2, search=image.SEARCH_EX):#匹配率85%更新目标
                image_data.draw_image(image_matching,0,0)
            img.draw_rectangle(results,thickness=2,color=(255,0,0))
            uart_bytes=str(results[0]+25-80)
            #print(results[0]+25-80)
            uart.write(uart_bytes+'\n')

        uart_read=uart.readchar()
        if uart_read==88 and  uart_read_last==82 : #received "RX"
            print("received RX\n")
            break
        uart_read_last=uart_read
        screen.display(img,x_size=320)
